#ifndef WALLFOLLOWER_H_
#define WALLFOLLOWER_H_

#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include "Driver.h"
#include "../utilities/Event.h"
#include "../utilities/ThreadSafeQueue.h"
#include "../utilities/Constants.h"
#include <string>
#include <math.h>       /* atan */
using namespace std;

#define MSG_QUEUE_SIZE 5

/**
 * Implements a wall following algorithm: \n
 * keeps indicated distance from wall, while driving at given speed
 */
class WallFollower {

private:

	// ROS management objects
	ros::NodeHandle m_node_handler;
	ros::Rate* m_loop_rate;
	ros::Rate* m_fast_loop_rate;
	ros::Publisher m_sensors_publisher;
	std_msgs::String m_message_to_publish;
	ros::Subscriber sub_sensors_frl;
	ros::Subscriber sub_sensors_bl;
	ros::Subscriber sub_sensors_back_us;
	ros::Subscriber sub_sensors_rl;
	ros::Subscriber sub_sensors_right_us;
	ros::Subscriber sub_sensors_ll;
	ros::Subscriber sub_sensors_left_us;
	ros::Subscriber sub_sensors_fl;

	// utility objects
	ThreadSafeQueue<Event> event_queue; // holds recent events
	Driver* m_driver; // object to move the robot
	double m_speed; // robot's speed while following wall
	double m_goalWallDist; // distance to keep from wall
	double m_sideDist; // distance measured from side sensor
	std::mutex m_mutex; // used for synchronized access to data

	void desubscribe();

	/**
	 * Maintains parallel driving at indicated distance from wall: \n
	 * If robot's distance exceeds the range bounded by the error tolerant,
	 * its driving direction is fixed
	 */
	void maintainDirection();

	/**
	 * Method is called if an event has been created- robot is either too close
	 * or too far from the wall.
	 * Uses sampled distance from wall to decide whether should turn towards the
	 * wall or away from it:
	 * calculates how much should turn in order to be parallel and how much should
	 * turn in order to keep right distance.
	 * If angles are at different direction, turns in vector's sum.
	 * If both are of the same direction, turns in the maximal angle among the two.
	 * After turning angle is calculated, it is scaled up because robot's architecture
	 * turns in radians per second, but event should be handled in TIME_FOR_EVENT microseconds
	 * @param currEvent current event to be popped out of event queue
	 */
	void fixDistance(const Event& currEvent);

	/**
	 * Calculates how much should turn (in radians) in order to fix
	 * robot's orientation, so it will be parallel to the wall.
	 * Calculated value does not guarantee that robot will be at correct
	 * distance from wall, but it will be parallel
	 * @param event event holding sensor's data to use for the calculation
	 * @return angle (radians) to fix tilt (negative angle means turn right)
	 */
	double align(const Event& event);

	/**
	 * Calculates how much should turn (in radians) in order to fix
	 * robot's distance from wall- turn towards the wall or away from the wall.
	 * Calculated value does not guarantee that robot will be parallel to the wall,
	 * but it will be at correct distance
	 * @param horizontalDist robot's current distance from wall
	 * @return correct turn (radians) towards or away from the wall
	 */
	double turnAngle(double horizontalDist);

	/**
	 * Returns distance sampled from side sensor.
	 * Method is synchronized because sensor's handler keeps updating variable
	 * @return newest distance sampled from side sensor
	 */
	double getSideDistance();

    /**
     * Sets current distance sampled from side sensor.
     * Method is synchronized because algorithm keeps reading variable
     * @param newDist newest distance sampled from side sensor.
     */
	void setSideDistance(double newDist);

public:

	/**
	 * Subscribes to relevant sensors for the wall following
	 * @param sensor index of relevant sensor (see \ref ../utilities/Constants.h)
	 * @param shouldSubscribe True if establishing subscription, False if cancel
	 * @param min_distance smaller distances sampled by the sensor cause notification
	 * @param max_distance greater distances sampled by the sensor cause notification
	 */
	void subscribeToSensor(sensors_names::ordinal sensor,
			bool shouldSubscribe, double min_distance, double max_distance);

	// handler methods- create events according to sampled data
	/**
	 * Callback for front laser's range. \n
	 * In this topic the SensorNode will publish distances from the laser.
	 * @param range RosMessage array of distances from laser
	 */
	void frontLaserHandler(const std_msgs::String::ConstPtr& msg);

	/**
	 * Callback for front laser's right section. \n
	 * In this topic the SensorNode will publish distances from the laser.
	 * @param range average value of the right section
	 */
	void frontRightLaserHandler(const std_msgs::String::ConstPtr& msg);

	/**
	 * Callback for right laser's range. \n
	 * In this topic the SensorNode will publish distances from the laser.
	 * @param range value sampled by the sensor
	 */
	void rightLaserHandler(const std_msgs::String::ConstPtr& msg);

	// used for exa frontLaserHandlermple of subscription, could be implemented for future use
	void backLaserHandler(const std_msgs::String::ConstPtr& msg);
	// used for example of subscription, could be implemented for future use
	void backUSHandler(const std_msgs::String::ConstPtr& msg);
	// used for example of subscription, could be implemented for future use
	void rightUSHandler(const std_msgs::String::ConstPtr& msg);
	// used for example of subscription, could be implemented for future use
	void leftUSHandler(const std_msgs::String::ConstPtr& msg);
	// used for example of subscription, could be implemented for future use
	void leftLaserHandler(const std_msgs::String::ConstPtr& msg);

	/**
	 * Constructor- initializes a new object to manage wall following
	 * @param driver pointer to object handling the robot's moves
	 * @param speed linear speed (meter/second) in which wall following is performed
	 * @param wallDistance distance (cm) to keep from wall
	 */
	WallFollower(Driver* driver, double speed, double wallDistance);

	/**
	 * Destructor- cancels subscription to sensors
	 */
	~WallFollower();

	/**
	 * Reads data from event queue and responds accordingly
	 */
	void follow();

	/**
	 * Follows wall for given time
	 * @param time_to_run how long should run (seconds)
	 */
	void follow(unsigned int time_to_run);
};

#endif
